import serial
import math
from functools import reduce

class STM32Communicator:
    def __init__(self, device="/dev/ttyUSB1", baudrate=115200):
        self.device = device
        self.baudrate = baudrate
        self.run = False
        
        # 初始化串口
        try:
            self.serial = serial.Serial(device, baudrate=baudrate, timeout=0.1)
            print(f"串口已打开: {device}, 波特率: {baudrate}")
        except Exception as e:
            print(f"串口打开失败: {e}")
            self.serial = None
        
    def set_running_status(self, status):
        """设置运行状态"""
        if status == 1:  # start
            self.run = True
        elif status == 2:  # pause
            self.run = False
        elif status == 3:  # resume
            self.run = True
        elif status == 4:  # stop
            self.run = False
        else:
            print(f"错误: 无法识别的控制码: {status}")
    
    def send_control_data(self, lat_err, yaw_err_rad):
        """
        直接发送控制数据
        
        参数:
            lat_err: 横向误差 (米)
            yaw_err_rad: 航向误差 (弧度)
        """
        if not self.run:
            print("运行控制暂停，不发送数据")
            return False
            
        if self.serial is None:
            print("串口未初始化")
            return False
            
        try:
            yaw_err_deg = self.rad2deg(yaw_err_rad)
            data = self.construct_msg(lat_err, yaw_err_deg)
            self.serial.write(data)
            # print(data)
            print(f"发送数据: 横偏误差={lat_err:.3f}m, 航向误差={yaw_err_deg:.3f}°")
            return True
        except Exception as e:
            print(f"发送数据失败: {e}")
            return False
    
    @staticmethod
    def rad2deg(rad):
        """弧度转角度"""
        return rad * 180 / math.pi
    
    @staticmethod
    def construct_msg(lat_err, yaw_err):
        """构造消息数据包"""
        def pack_data(num):
            s0 = 0 if num > 0 else 1
            n = math.fabs(num)
            l1 = int(n / 100) % 10
            h2 = int(n / 10) % 10
            l3 = int(n) % 10
            h4 = int(n * 10) % 10
            h5 = int(n * 100) % 10
            return [s0 << 4 | l1, h2 << 4 | l3, h4 << 4 | h5]

        # 限制误差范围
        lat_err = 999.99 if lat_err > 999.99 else -999.99 if lat_err < -999.99 else lat_err
        yaw_err = 999.99 if yaw_err > 999.99 else -999.99 if yaw_err < -999.99 else yaw_err

        data = [2] + pack_data(lat_err) + pack_data(yaw_err)
        xor_chk = reduce(lambda x, y: x ^ y, data, 0)
        data = [1] + [42] * 7 + data + [xor_chk] + [42] * 8
        assert len(data) == 24, "数据长度错误，应该是24字节"

        return bytes(data)
    
    def close(self):
        """关闭串口"""
        if self.serial:
            self.serial.close()
            print("串口已关闭")

# #!/usr/bin/env python3

# import math
# from functools import reduce

# import rclpy
# from rclpy.node import Node
# from std_msgs.msg import Float64MultiArray, UInt64
# import serial


# class PubToSTM32(Node):
#     def __init__(self, name):
#         super().__init__(name)
#         self.declare_parameter("device", "/dev/ttyUSB0")
#         device = self.get_parameter("device").get_parameter_value().string_value
#         self.car_error = None
#         self.local_coord = None
#         self.run = False

#         self.serial = serial.Serial(device, baudrate=115200, timeout=0.1)
#         self.sub_car_error = self.create_subscription(Float64MultiArray, "/car_err", self.get_car_error_callback, 1)
#         self.sub_local_coordination = self.create_subscription(
#             Float64MultiArray, "/local_info", self.get_local_coord_callback, 1
#         )
#         self.sub_running_status = self.create_subscription(
#             UInt64, "/running_control", self.get_running_status_callback, 1
#         )
#         self.timer = self.create_timer(0.2, self.timer_callback)

#     def get_car_error_callback(self, msg):
#         self.car_error = msg.data

#     def get_local_coord_callback(self, msg):
#         self.local_coord = msg.data

#     def get_running_status_callback(self, msg):
#         status = msg.data
#         if status == 1:  # start
#             self.run = True
#         elif status == 2:  # pasue
#             self.run = False
#         elif status == 3:  # resume
#             self.run = True
#         elif status == 4:  # stop
#             self.run = True
#         else:
#             self.get_logger().error(f"get unrecoginized control code: {status}")

#     def timer_callback(self):
#         if self.car_error is None:  # or self.local_coord is None:
#             self.get_logger().debug(f"now msg is not received")
#         else:
#             if self.run:
#                 lat_err = self.car_error[4]
#                 yaw_err = self.rad2deg(self.car_error[5])
#                 data = self.construct_msg(lat_err, yaw_err)
#                 # self.get_logger().info(f"{data}")
#                 self.serial.write(data)
#             else:
#                 self.get_logger().info("running control is pause")

#     @staticmethod
#     def rad2deg(rad):
#         return rad * 180 / math.pi

#     @staticmethod
#     def construct_msg(lat_err, yaw_err):
#         def pack_data(num):
#             s0 = 0 if num > 0 else 1
#             n = math.fabs(num)
#             l1 = int(n / 100) % 10
#             h2 = int(n / 10) % 10
#             l3 = int(n) % 10
#             h4 = int(n * 10) % 10
#             h5 = int(n * 100) % 10
#             return [s0 << 4 | l1, h2 << 4 | l3, h4 << 4 | h5]

#         lat_err = 999.99 if lat_err > 999.99 else -999.99 if lat_err < -999.99 else lat_err
#         yaw_err = 999.99 if yaw_err > 999.99 else -999.99 if yaw_err < -999.99 else yaw_err

#         data = [2] + pack_data(lat_err) + pack_data(yaw_err)
#         xor_chk = reduce(lambda x, y: x ^ y, data, 0)
#         data = [1] + [42] * 7 + data + [xor_chk] + [42] * 8
#         assert len(data) == 24, "data length error, it should be 8."

#         return bytes(data)


# def main():
#     rclpy.init()
#     rclpy.spin(PubToSTM32("to_stm32"))
#     rclpy.shutdown()


# if __name__ == "__main__":
#     main()
